#pragma once

#include "rclcpp/rclcpp.hpp"
#include <agv_actions/PeripheralAction.h>
#include <agv_msgs/msg/v_action.hpp>
#include <actionlib/server/simple_action_server.h>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/agv_info.hpp>
#include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/agv_event_status.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/v_load.hpp>
#include <vector>
#include <thread>
#include <mutex>
#include <agv_peripheral_controller/roller/roller_controller.h>
#include <agv_peripheral_controller/peripheral_event.h>
#include <unordered_map>
// 动作列表
#define ORDER_PICK "pick"          // 动作上升指令
#define ORDER_DROP "drop"          // 动作下降指令
#define ORDER_PICK_OLD_VERSION "1" // 兼容旧版本动作指令动作上升指令
#define ORDER_DROP_OLD_VERSION "2" // 兼容旧版本动作下降指令

// 参数列表
// 站点类型
#define STATION_TYPE_KEY "stationType"       // 站点类型-key
#define TYPE_DYN_VALUE "active_conveyor"     // 站点类型值-value1
#define TYPE_STATIC_VALUE "passive_conveyor" // 站点类型值-value2
// 对接边缘
#define SIDE_TYPE_KEY "side" // 对接边缘-key
#define SIDE_FRONT "front"   // 对接边缘-前
#define SIDE_BACK "back"     // 对接边缘-后
#define SIDE_LEFT "left"     // 对接边缘-左
#define SIDE_RIGHT "right"   // 对接边缘-右
// 设备编号
#define DEVICE_ID_KEY "deviceId" // 设备运行编号

// 设备1:bit0  设备2:bit1  设备3:bit2 ...组合位方式
namespace peripheral_roller
{
  // 外设逻辑层
  class CPeripheral
  {
  private:
    // 支持的操作指令
    enum class OpCode
    {
      NO_OP = 0,
      PICK = 1, // 手柄可操作码
      DROP = 2,
      ReadyStart =99,
      EMG = 100
      // CLOCKWISE_ROTATION,
    };
    // 自动模式下操作结果返回值
    enum OpResult
    {
      SUCCESS,
      FAILED,
      CANCELED,
      OTHER
    };

    // 自动运行时阶段
    enum AutoRunStage
    {
      STAGE_IDLE,
      STAGE_RUN,
      STAGE_END,
      STAGE_FAILED
    };

    // SideType到字符串的映射
    const std::unordered_map<std::string, SideType> sideTypeMap = {
        {SIDE_FRONT, SideType::Front},
        {SIDE_BACK, SideType::Back},
        {SIDE_LEFT, SideType::Left},
        {SIDE_RIGHT, SideType::Right}};

    // ros下的动作服务端类型定义
    typedef actionlib::SimpleActionServer<agv_actions::PeripheralAction> ACTION_SERVER_TYPE;

    // ros下动作实时反馈类型
    typedef agv_actions::PeripheralFeedback ACTION_FEED_BACK_TYPE;

    // ros下动作结果类型
    typedef agv_actions::PeripheralResult ACTION_RESULT_TYPE;

  public:
    // 默认构造函数
    CPeripheral(const std::string &name);

    // 析构函数
    ~CPeripheral();

    // 初始化函数
    bool init();

    // 运行函数
    void running();

  private:
    // 私有节点句柄
    rclcpp::Node m_priNh;

    // 服务端对象
    ACTION_SERVER_TYPE m_server;

    // 动作反馈对象
    ACTION_FEED_BACK_TYPE m_actionFeedBack;

    // 动作结果对象
    ACTION_RESULT_TYPE m_actionResult;

    // 设备控制对象
    CRollerController m_roller;

    // 当前的Action 动作
    agv_msgs::msg::V_action m_curAction;

    // 手柄指令订阅
    ros::Subscriber m_periTelopSub;

    // 手自动模式订阅
    ros::Subscriber m_agvInfoSub;

    // 事件订阅:获取急停信号事件
    ros::Subscriber m_eventSub;

    // 外设状态发布
    ros::Publisher m_periStatusPub;

    // 当前操作模式
    uint8_t m_curMode;

    // 控制模式锁
    std::mutex m_modeMtx;

    // 手动操作码
    CDataTimeStamp<OpCode> m_manulOpCode;

    // 自动操作码
    CDataTimeStamp<OpCode> m_autoOpCode;

    // 自动运行阶段
    AutoRunStage m_curStage;

    // 事件发布
    PeripheralEvent m_event;

    // 参数列表
    CParameters m_params;

  private:
    // 复位相关变量和状态
    void reset();

    // 加载参数
    void loadParam();

    // 动作服务端接收动作指令回调函数
    void goalCB();

    // 抢占式回调函数：这个回调可能在动作服务器需要取消或中止正在进行的操作时被调用
    void preemptCB();

    // 外设手柄指令回调函数
    void periphTeleopCtlCB(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);

    // 操作模式回调函数
    void agvInfoCB(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg);

    // 设置实时反馈数据
    void setFeedBack(OpCode code, float percent);

    // 设置反馈结果
    void setResult(OpResult result, std::string msg);

    // 设置取消任务反馈
    void setPreempt(OpResult result, std::string msg);

    // 发布者是否有效
    bool publiserValid(const ros::Publisher &pub);

    // 外设状态发布
    void periphStatusPub();

    // 外设事件发布
    void eventPub(bool occur, const std::string &msg);

    // 找寻参数列表里的站点类型
    bool findSpecialParam(agv_msgs::msg::V_action action, CParameters &outParams);

    // 执行操作
    bool execOp(OpCode code);

    // 自动运行循环
    void autoRun();

    // 手动运行循环
    void manulRun();

    // 其他操作模式运行
    void otherRun();
  };
} // agv_peripheral